﻿using System;

namespace CoreRay
{
    public static class MathUtility
    {
        //-------------------------------------------------------------------------------------
        /// <summary>
        /// PI Constant.
        /// </summary>
        //-------------------------------------------------------------------------------------

        public const float PI = (float) System.Math.PI;


        //-------------------------------------------------------------------------------------
        /// <summary>
        /// 2 * PI constant.
        /// </summary>
        //-------------------------------------------------------------------------------------

        public const float PI2 = (float) System.Math.PI * 2.0f;


        //-------------------------------------------------------------------------------------
        /// <summary>
        /// PI / 2 contant.
        /// </summary>
        //-------------------------------------------------------------------------------------

        public const float PIOver2 = (float) System.Math.PI * 0.5f;


        //-------------------------------------------------------------------------------------
        /// <summary>
        /// 1 / PI constant.
        /// </summary>
        //-------------------------------------------------------------------------------------

        public const float OneOverPI = 1.0f / MathUtility.PI;


        //-------------------------------------------------------------------------------------
        /// <summary>
        /// 1 / 2 * PI constant.
        /// </summary>
        //-------------------------------------------------------------------------------------

        public const float OneOverPI2 = 1.0f / MathUtility.PI2;


        //-------------------------------------------------------------------------------------
        /// <summary>
        /// Converts a degrees angle into a radians angle.
        /// </summary>
        /// <param name="angle">Angle to convert.</param>
        /// <returns>Angle in radians.</returns>
        //-------------------------------------------------------------------------------------

        public static float DegreesToRadians(float angle)
        {
            return angle * ((float) System.Math.PI / 180.0f);
        }


        //-------------------------------------------------------------------------------------
        /// <summary>
        /// Converts a radians angle into a degrees angle.
        /// </summary>
        /// <param name="angle">Angle to convert.</param>
        /// <returns>Angle in degrees.</returns>
        //-------------------------------------------------------------------------------------

        public static float RadiansToDegrees(float angle)
        {
            return angle * (180.0f / (float) System.Math.PI);
        }


        //-------------------------------------------------------------------------------------
        /// <summary>
        /// Same as System.Math.Acos but if angle is out of range, the result is clamped.
        /// </summary>
        /// <param name="angle">Input angle.</param>
        /// <returns>Acos the angle.</returns>
        //-------------------------------------------------------------------------------------

        public static float SafeAcos(float angle)
        {
            if(angle <= -1.0f)
            {
                return MathUtility.PI;
            }

            if(angle >= 1.0f)
            {
                return 0.0f;
            }

            return (float) System.Math.Acos(angle);
        }


        //-------------------------------------------------------------------------------------
        /// <summary>
        /// Wraps an angle in range [-pi..pi] by adding the correct multiple of 2 PI.
        /// </summary>
        /// <param name="angle">Angle to wrap.</param>
        /// <returns>Wraped angle.</returns>
        //-------------------------------------------------------------------------------------

        public static float WrapPI(float angle)
        {
            angle += MathUtility.PI;
            angle -= (float) System.Math.Floor(angle * MathUtility.OneOverPI2) * MathUtility.PI2;
            angle -= MathUtility.PI;

            return angle;
        }


        public static T Clamp<T>(T value, T minValue, T maxValue) where T : IComparable<T>
        {
            if (value.CompareTo(maxValue) > 0)
            {
                return maxValue;
            }

            else if (value.CompareTo(minValue) < 0)
            {
                return minValue;
            }

            return value;
        }

        public static void CoordinateSystem(Vector3 vector1, out Vector3 vector2, out Vector3 vector3) 
        {
            if (Math.Abs(vector1.X) > Math.Abs(vector1.Y)) 
            {
                float invLen = 1.0f / (float) Math.Sqrt(vector1.X * vector1.X + vector1.Z * vector1.Z);
                vector2 = new Vector3(-vector1.Z * invLen, 0.0f, vector1.X * invLen);
	        }

	        else 
            {
                float invLen = 1.0f / (float) Math.Sqrt(vector1.Y * vector1.Y + vector1.Z * vector1.Z);
                vector2 = new Vector3(0.0f, vector1.Z * invLen, -vector1.Y * invLen);
	        }

            vector3 = Vector3.CrossProduct(vector1, vector2);
        }

        public static QuadraticResult? Quadratic(float variableA, float variableB, float variableC) 
        {
            float result0;
            float result1;

	        //Find quadratic discriminant
	        float discrim = variableB * variableB - 4.0f * variableA * variableC;

            if(discrim < 0.0f)
            {
                result0 = 0.0f;
                result1 = 0.0f;
                return null;
            }

	        float rootDiscrim = (float) Math.Sqrt(discrim);
	
            //Compute quadratic _t_ values
	        float q;

            if(variableB < 0.0f)
            {
                q = -0.5f * (variableB - rootDiscrim);
            }

            else
            {
                q = -0.5f * (variableB + rootDiscrim);
            }

	        result0 = q / variableA;
	        result1 = variableC / q;

	        if (result0 > result1) 
            {
                float temp = result0;

                result0 = result1;
                result1 = temp;
            }

            return new QuadraticResult(result0, result1);
        }
    }

    public struct QuadraticResult
    {
        public QuadraticResult(float result0, float result1) : this()
        {
            this.Result0 = result0;
            this.Result1 = result1;
        }

        public float Result0
        {
            get;
            private set;
        }

        public float Result1
        {
            get;
            private set;
        }
    }
}

